clear;
clc;
MapName='test_1.map';
disp('A Star Path Planing start!!')
MapInfo=read_data(MapName);
map.X=MapInfo{1};map.Y=MapInfo{2}; %%代表我们要画一个地图的长和宽
map.start=[MapInfo{4},MapInfo{5}];  %起始点 注意必须在地图范围内
map.goal=[MapInfo{6},MapInfo{7}];  %目标点 注意必须在地图范围内

[obstacle_x,obstacle_y]=find(MapInfo{3}==1);
obstacle=[obstacle_x,obstacle_y];

[path,~]=AStar3(obstacle,map);%A*算法
maps_type=zeros(map.X,map.Y);

for FoundObsIndex=1:length(obstacle(:,1))
        maps_type(obstacle(FoundObsIndex,1),obstacle(FoundObsIndex,2))=2; % 默认是未发现的障碍物
end

FinalPlot(map,obstacle,maps_type,path)

function FinalPlot(map,obstacle,maps_type,path)

plot(map.start(1),map.start(2),'r*');hold on
plot(map.goal(1),map.goal(2),'b*');hold on
detective_area=20;

path(all(path==0,2),:)=[];

FoundedList=[];
pic_num=1;
for current_place=1:length(path(:,1))
%     pause(0.5)
    % 将所有障碍物填充
    for FoundObsIndex=1:length(obstacle(:,1))
        maps_type(obstacle(FoundObsIndex,1),obstacle(FoundObsIndex,2))=2; % 默认是未发现的障碍物
    end

    % 将所有发现的障碍物填充
    if ~isempty(FoundedList)
    for FoundObsIndex=1:length(FoundedList(:,1))
        maps_type(FoundedList(FoundObsIndex,1),FoundedList(FoundObsIndex,2))=1; % 发现的障碍物
    end
    end

    % 视线以内的障碍物  
    DetectiveObsIndex=( (obstacle(:,1)-path(current_place,1)).^2+...
        (obstacle(:,2)-path(current_place,2)).^2 ) < detective_area^2;
    DetectiveObsList=DetectiveObsIndex.*obstacle;
    DetectiveObsList(all(DetectiveObsList==0,2),:)=[];
    for NewObsIndex=1:length(DetectiveObsList(:,1))
        maps_type(DetectiveObsList(NewObsIndex,1),DetectiveObsList(NewObsIndex,2))=3; % 视线内的障碍物用5来表示
    end
    
    FoundedList=[FoundedList;DetectiveObsList];
    FoundedList=unique(FoundedList,'rows');
    PlotGrid2(map,maps_type,[1 1 1;0 0 0;0.6,0.6,0.6;1 0 0]);
    ax = gca;
%     ax.XDir = 'reverse';
    ax.YDir = 'reverse';% 倒转y轴
    line(path(:,1),path(:,2),'Color','green','LineWidth',2);hold on
    plot(path(current_place,1),path(current_place,2),'r*');
%     axis([100.5 -0.5,100.5 -0.5])
    
     F=getframe(gcf);%捕获图窗窗口的内部区域，不包括菜单和工具栏。
    I=frame2im(F);%从单个影片帧 F 返回真彩色 (RGB) 图像。
    [I,map]=rgb2ind(I,256);
    if pic_num == 1
        imwrite(I,map,'test.gif','gif', 'Loopcount',inf,'DelayTime',0.1);
    else
        imwrite(I,map,'test.gif','gif','WriteMode','append','DelayTime',0.1);
    end
    pic_num = pic_num + 1;
end
end


